/* **************************************************************** */
/* GROUP:       Cedric Wens, Shana Van Dessel, Thomas Van Winckel,  */
/*              Jasper Henne & Alessandro Vasalli                   */
/* **************************************************************** */
/* Authors:     Shana Van Dessel & Cedric Wens                      */
/* Document:    Robocup.h                                           */
/* Description: Main - aanmaken van objecten van verschillende      */
/*              klassen en aanroepen van de verschillende functies  */
/*              algemene functies                                   */
/* **************************************************************** */

#include <iostream>
#define DEBUG 1
#include "Robocup.h"
#include "Reciever.h"
#include "Robot.h"


using namespace std;

int main()
{
    int thStop = 50;           // stoppen
    int thNear = 200;       // vertragen

    Reciever rec;               // objecten aanmaken
	Robot robot;
	Thing can;
	Thing home;

    Robocup cup;                // object aanmaken van hoofdklasse
	cup.setReceiver(rec);       //   en objecten toevoegen
	cup.setRobot(robot);
	cup.setCan(can);
	cup.setHome(home);

    cout << "INIT" << endl;
    cup.initThings();
    cup.startRobot();

    #ifdef DEBUG
        cout << "MAIN" << endl;
        cout << "robot(" << cup.getRobot().getXPos() << "," << cup.getRobot().getYPos() << "," << cup.getRobot().getAngle() << ")"<< endl;
        cout << "doel(" << cup.getCan().getXPos() << "," << cup.getCan().getYPos() << ")"<< endl;
        cout << "gar(" << cup.getHome().getXPos() << "," << cup.getHome().getYPos() << ")"<< endl << endl;
    #endif

    cout << "RECEIVE" << endl;
    struct Info data = cup.getReceiver().recieve();    // udp-pakket ophalen

    #ifdef DEBUG
        cout << "MAIN" << endl;
        cout << "robot(" << data.robx << "," << data.roby << "," << data.robhoek << ")"<< endl;
        cout << "doel(" << data.doelx << "," << data.doely << ")"<< endl;
        cout << "gar(" << data.garx << "," << data.gary << ")"<< endl << endl;
    #endif

    cout << "UPDATE" << endl;
    cup.updateThings(data);                     	// voorwerpen initialiseren
    cup.prepareRobot(true);                             // afstand en hoek naar doel berekenen

    #ifdef DEBUG
        cout << "MAIN" << endl;
        cout << "robot(" << cup.getRobot().getXPos() << "," << cup.getRobot().getYPos() << "," << cup.getRobot().getAngle()
               << "," << cup.getRobot().getDistance() << ","<< cup.getRobot().getRotation() << ")" << endl;
        cout << "doel(" << cup.getCan().getXPos() << "," << cup.getCan().getYPos() << ")"<< endl;
        cout << "gar(" << cup.getHome().getXPos() << "," << cup.getHome().getYPos() << ")"<< endl << endl;
    #endif
    
    cup.getRobot().catchCan();
    cup.getRobot().releaseCan();
    
    do
    {
        cout << "RECEIVE" << endl;
            data = rec.recieve();                 // udp-pakket ophalen

        cout << "UPDATE" << endl;
        cup.updateThings(data);                             // coordinaten opslaan
        cup.prepareRobot(true);                                 // afstand en hoek naar doel berekenen

        if(cup.getRobot().getDistance() <= thStop)
        {
            cout << "We zijn er!" << endl;
            break;
            
        }
        
        cup.moveRobot(false);
        
        /*if(cup.getRobot().getDistance() > thNear)
        {
            cout << "Het is nog ver hoor..." << endl;
            cup.moveRobot(false);
        }
        else
        {
            cout << "We zijn er bijna ..." << endl;
            cup.moveRobot(true);
        }*/
        
        cout << "MOVE"<< endl;
    }
    while(1);
    
    cout << "Grijpen maar !" << endl;
    //cup.getRobot().catchCan();
    cup.getRobot().catchCan();
    
    cout << "UPDATE" << endl;
    cup.updateThings(data);                     	// voorwerpen initialiseren
    cup.prepareRobot(false);                             // afstand en hoek naar doel berekenen

    do
    {
        cout << "RECEIVE" << endl;
        data = rec.recieve();                 // udp-pakket ophalen
        
        cout << "UPDATE" << endl;
        cup.updateThings(data);                             // coordinaten opslaan
        cup.prepareRobot(false);                                 // afstand en hoek naar doel berekenen
        
        if(cup.getRobot().getDistance() <= thStop)
        {
            cout << "Lossen maar !" << endl;
            break;
        }
        
        cout << "Verder graag." << endl;
        cup.moveRobot(false);
        
        cout << "MOVE"<< endl;
    }
    while(1);
   
    cup.getRobot().releaseCan();
    cup.getRobot().releaseCan();

    cup.stopRobot();
    #ifdef DEBUG
		cout << "*** STOP ***"<< endl;
	#endif

    return 0;
}
